/*
 * NeuroAuto.hh
 *
 *  Created on: Jan 28, 2009
 *      Author: robotics
 */
#ifndef NEUROAUTO_H_
#define NEUROAUTO_H_

#include <iostream.h>
#include "math.h"

#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "TrackAPI.h" 
#include "WPILib.h"
#include "RobodoxTimer.h"
#include "RobodoxEncoder.h"

/*
 * Description: Autonomous code for Robodox 2009 FRC robot, the Neurologist
 */
class NeuroAuto{
	
public:
	float tol;
	float gain;
	float v_robot;
	static const float gearRatio = 12.5;			//Andy Mark toughbox ratio
	static const float Ke = 0.021142234;						//CIM-001 Motor back EMF constant in volts/(radian/second)
	//static const float maxMotorSpeed;
	static float maxWheelRate;
	
	/*
	 * Default constructor
	 */
	NeuroAuto();
	NeuroAuto(Jaguar *l, Jaguar *r, RobodoxEncoder *lg, RobodoxEncoder *rg, RobodoxEncoder *rref, RobodoxEncoder *lref, RobodoxTimer *rt, Gyro *g); //YAP: took out Encoder lr/rr
	
	void TractionControl();
	void RandomDrive();
	float randRange(float min, float max);
	void CenterCircling();
	void CornerCircling();
	void AroundDrive(int position);
	void GoForward(int inches);
	void GoForward(double tim);
	void Turn(int degrees);
	void Stop();
	
	/*
	 * Default destructor
	 */
	~NeuroAuto();

protected:

private:
	
	Jaguar *leftMotor;
	Jaguar *rightMotor;
	
	RobodoxEncoder *leftGearbox;
	RobodoxEncoder *rightGearbox;
	//Encoder *leftRear;
	//Encoder *rightRear;
	RobodoxEncoder *rightRef;
	RobodoxEncoder *leftRef;
	
	RobodoxTimer *time;
	
	Gyro *g1;
	
	float sum;
	float *timeArray;
	float *speedArray;
	int arrayLength;
	int i, autoIndex;
	bool begin;
};

#endif
